from Modules.foward_kinematics import aubo_FK
from Modules.get_distance import *
from Modules.robot_class import *


def auboi5_collision_check(joint_point: Joint_point, cube_obstacle: Cube_obstacle) -> bool:
    # Get the coordinate origin of DH
    [T1, T2, T3, T4, T5, T6] = aubo_FK(joint_point)
    # check link1:
    o1 = Point([0, 0, 0])
    o2 = Point([0, 0, 0.14])
    d1 = distance_line_to_cube_obstacle(o1, o2, cube_obstacle)
    # print("d1",d1)
    if d1 <= 0.075:
        link1_collision_hapen = True
        # print("link1 collison")
    else:
        link1_collision_hapen = False

    # check link2:
    o1 = Point([float(T2[0, 3]), float(T2[1, 3]), float(T2[2, 3])])
    o2 = Point([float(T3[0, 3]), float(T3[1, 3]), float(T3[2, 3])])
    d2 = distance_line_to_cube_obstacle(o1, o2, cube_obstacle)
    # print("d2",d2)
    # print("o2",o1.p)
    # print("o3",o2.p)
    if d2 <= 0.0872:
        link2_collision_hapen = True
        # print("link2 collison")
    else:
        link2_collision_hapen = False

    # check link3:
    o1 = Point([float(T3[0, 3]), float(T3[1, 3]), float(T3[2, 3])])
    o2 = Point([float(T4[0, 3]), float(T4[1, 3]), float(T4[2, 3])])
    d3 = distance_line_to_cube_obstacle(o1, o2, cube_obstacle)
    # print("d3",d3)
    # print("o3",o1.p)
    # print("o4",o2.p)
    if d3 <= 0.15:
        link3_collision_hapen = True
        # print("link3 collison")
    else:
        link3_collision_hapen = False

    # check link4 + link5 + link6:
    o = Point([T5[0, 3], T5[1, 3], T5[2, 3]])
    d4 = distance_point_to_cube_obstacle(o, cube_obstacle)
    # print("d4",d4)
    # print("o5",o.p)
    if d4 <= 0.155:
        link4_collision_hapen = True
        # print("link4 collison")
    else:
        link4_collision_hapen = False

    if link1_collision_hapen or link2_collision_hapen or link3_collision_hapen or link4_collision_hapen:
        return True  # collision hapen
    else:
        return False  # safe

# joint_point = Joint_point([0, 0, 0, 0, 0, 0])
# obstacle_center = Point([0, -0.315, 0.197])
# obbstacle = Cube_obstacle(obstacle_center, l=0.1, w=0.1, h=0.1)
# result = auboi5_collision_check(joint_point, obbstacle)
# print(result)
